#ifndef __KINEMATICS_H__
#define __KINEMATICS_H__

#include <Arduino.h>
typedef struct
{
    float per_pulse_distance;  // 单个脉冲对应的距离
    int64_t motor_speed;       // 当前电动机速度mm/s
    int64_t last_encoder_tick; // 上次电动机编码器的读数
} motor_param_t;

typedef struct{
    float x;
    float y;
    float angle;
    float linear_speed;
    float angle_speed;
}odom_t;

// 定义一个类用于处理机器人运动学
class Kinematics
{
private:
    /* data */
    odom_t odom_;
    motor_param_t motor_param_[2];
    uint64_t last_update_time; // 单位ms
    float wheel_distance_;

public:
    Kinematics(/* args */) = default;
    ~Kinematics() = default;

    void set_motor_param(uint8_t id, float_t per_pulse_distance);
    void set_wheel_distance(float wheel_distance);

    /*运动学计算*/
    void kinematic_inverse(float linear_speed, float angle_speed,
                           float &out_left_speed, float &out_right_speed);
    void kinematic_forword(float left_speed, float right_speed,
                           float &out_linear_speed, float &out_angle_speed);

    /*更新电动机速度和编码器数据*/
    void update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick);
    /*获取电动机速度*/
    int16_t get_motor_speed(uint8_t id);
    /*更新里程计数据*/
    void update_odom(uint16_t dt);
    /*获取里程计数据*/
    odom_t &get_odom(); //返回值是一个引用
    /*将角度转换到-M_PI到M_PI的范围内*/
    static void TransAngleInPI(float angle, float &out_angle);
};

#endif //__KINEMATICS_H__